#include <opencv2/opencv.hpp>
//
//cv::Mat test;
//edgePreserveFilter(im_R_rgb_enhanced, disp_raw, test, 10, 40);

namespace tcl_depth_image {
	void edgePreserveFilter(const cv::Mat &guided, cv::Mat &input, cv::Mat &output, int radius, int thresh)
	{
		if (output.empty())
		{
			input.copyTo(output);
		}
		cv::Mat input_tmp, guided_tmp;
		cv::copyMakeBorder(input, input_tmp, radius, radius, radius, radius, BORDER_CONSTANT, cv::Scalar(0));
		cv::copyMakeBorder(guided, guided_tmp, radius, radius, radius, radius, BORDER_CONSTANT, cv::Scalar(0, 0, 0));

		int w = input_tmp.cols - radius, h = input_tmp.rows - radius;
#pragma omp parallel for
		for (int i = radius; i < h; i++)
		{
			int x_beg, y_beg, x_end, y_end;
			const uchar* g_ptr0 = guided_tmp.ptr<uchar>(i);
			uchar* o_ptr = output.ptr<uchar>(i - radius);
			int a, b, c;
			int step0;
			int diff0;
			for (int j = radius; j < w; j++)
			{
				step0 = j * 3;
				a = g_ptr0[step0], b = g_ptr0[step0 + 1], c = g_ptr0[step0 + 2];
				if (j != radius)
				{
					diff0 = std::abs(g_ptr0[step0 - 3] - a) + std::abs(g_ptr0[step0 - 2] - b) + std::abs(g_ptr0[step0 - 1] - c);
					if (diff0 < 8)
					{
						o_ptr[j - radius] = o_ptr[j - radius - 1];
						continue;
					}

				}
				x_beg = j - radius;
				y_beg = i - radius;
				x_end = j + radius;
				y_end = i + radius;

				float sum = 0.0;
				int count = 0;
				for (int m = y_beg; m <= y_end; m += 3)
				{
					const uchar* g_ptr = guided_tmp.ptr<uchar>(m);
					uchar* i_ptr = input_tmp.ptr<uchar>(m);
					int step_;
					for (int n = x_beg; n <= x_end; n += 3)
					{
						step_ = n * 3;
						int diff = std::abs(g_ptr[step_] - a) + std::abs(g_ptr[step_ + 1] - b) + std::abs(g_ptr[step_ + 2] - c);
						if (diff < thresh)
						{
							sum += i_ptr[n];
							count++;
						}
					}
				}
				if (count != 0)
					o_ptr[j - radius] = (uchar)(sum / count);
			}
		}
	}
};

